/**********************************************************
 *
 *  Copyright (c) 2003  SeikoEpson Inc
 *  All Rights Reserved
 *
 *  File name : f_sqrt.c
 *  Function  :
 *        f_sqrt function returns a root value of given value.
 *        This file is copied from math.lib of 
 *                  CC33 tool(CC33v40.exe + CCPA45.exe, 
 *                  math of ansilib33v25 )
 *
 *  original file's Revision  :
 *      2000/02/14    first release                 M.Igarashi
 *  Revision  :
 *      2003/04/08    IrumaSoft M.Takeishi   1.st design
 *
 **********************************************************/
#include <f_math.h>
#include <f_smcvals.h>

// ALGORITHM
// 1.devide x into xfrac and xexp
// 2.normalize |xfrac| < 2^6
// 3.calculate sqrt( x ) by Newton's Method 
//     xfrac = ( x/xfrac + xfrac ) / 2
//
 
//  the architecture of single floating point
//
//   0 1          23 22            32bit
//   -----------------------------------
//  | |   exponent  |  fraction         |
//   -----------------------------------
//  |               |                   |
//  |      8bits          23 bits       |
//  |            lower word             |
//
//         bit   31         sign bit         (  1 bit  )
//             23 - 30      exponent part    (  8 bits )
//              0 - 22      fraction part    ( 23 bits )
//
//

float f_sqrt(float sfX){

	long lX;
	unsigned long ulx;
	int	iXexp,iTemp;
	float	sfXfrac,sfRet,sfTemp;
	unsigned long ulNaNData;

	F_GETW(lX,sfX);		// get argument
	
	ulx = lX&0x7fffffff;    // mask sin bit

	// error check
	if( (lX&0x80000000) !=0 || ( ulx == 0x0 ) ){
		if( ulx != 0x0 ){
			errno = EDOM;
			sfRet = f_NAN._F;
		}else{
			sfRet = 0.0f;		// sqrt(0) =0
		}
		return sfRet;
	}

	if ( ulx == 0x7f800000 )  {
		/* Is INF */
		errno = ERANGE;
		return sfX;
	}
	ulNaNData = f_NAN.st._LL;
	if ( ( ulx & ulNaNData ) == ulNaNData ) {
		/* Is NaN? */
		errno = EDOM;
		return f_NAN._F;
	}


	sfXfrac = f_frexp(sfX,&iXexp);
	if((iXexp&0x1) !=0){
		sfXfrac *= 2.0f;
		iXexp--;
	}
	
	// Newton iteration
	
	sfXfrac=0.5f*(1.0f+sfXfrac);     //(x+1.0)/2       initialize Newton iteration
	
	if(iXexp>=0){
		while(1){
			if(iXexp>50){               // normalize iXexp<50
				sfXfrac=sfXfrac*3.3554432e+7f;     // xfrac*2^25  iXexp is 32 bit
				iXexp=iXexp-50;
			}else{
				break;
			}
		}
		iTemp=iXexp/2;
		sfTemp=1L<<iTemp;
		sfXfrac=sfXfrac*sfTemp;                 // normalize sfXfrac*2^6
	}else{ 
		while(1){
			if(iXexp<-50){              // normalize iXexp>-50
				sfXfrac=sfXfrac/3.3554432e+7f;				//  xfrac/(2^25)  iXexp is 32 bit
				iXexp=iXexp+50;
			}else{
				break;
			}
		}
		iTemp=-iXexp/2;
		sfTemp=1L<<iTemp;
		sfXfrac=sfXfrac/sfTemp; 	// normalize dfXfrac*2^-6
	}
	
	for(iXexp=0;iXexp<=5;iXexp++){		// calc last Newton iteration
		sfTemp=sfX/sfXfrac;
		sfTemp+=sfXfrac;
		sfTemp*=0.5f;
		sfXfrac=sfTemp;
	}
	
	sfRet = sfXfrac;
	return sfRet;
	
}
